//
// Created by breeze on 23/3/31.
//
#include <cmath>

#include "func.h"


double Distance(double x1, double y1, double x2, double y2)
{
    return sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
}

double AngleDiff(double a1, double a2)
{
    double diff = a2 - a1;
    while (diff > M_PI) diff -= 2 * M_PI;
    while (diff < -M_PI) diff += 2 * M_PI;
    return diff;
}